#include "laser.h"
#include "kalman.h"
#include "cmsis_os2.h"

laser_t __shooter_laser, *shooter_laser = &__shooter_laser;
laser_t __chassis_laser1, *chassis_laser1 = &__chassis_laser1;

static float laser_update_dist(laser_handle_t laser)
{
    uint32_t sum = 0;
    for (int i = 0; i < laser->ave_count; i++)
    {
        sum += laser->adc_buffer[i * laser->adc_split_count + laser->adc_split_offset];
    }
    float ave = (float)sum / laser->ave_count;
    float dist = ave * laser->k_param + laser->b_param;
    laser->dist = Kalman_GetOutput(&laser->kal, dist);
    return laser->dist;
}

void laser_task(void *argument)
{
    for (;;) 
    {
        laser_update_dist(shooter_laser);
        laser_update_dist(chassis_laser1);
        osDelay(5);
    }   
}

void laser_init(laser_handle_t laser, const laser_init_t *init)
{
    laser->adc_buffer = init->adc_buffer;
    laser->adc_buffer_size = init->adc_buffer_size;
    laser->adc_split_count = init->adc_split_count;
    laser->adc_split_offset = init->adc_split_offset;
    laser->k_param = init->k_param;
    laser->b_param = init->b_param;
    laser->ave_count = init->adc_buffer_size / init->adc_split_count;

    laser->kal.A = 1;
    laser->kal.B = 0;
    laser->kal.Q = 0.01;
    laser->kal.R = 37.1160;
    laser->kal.kal_out = 0;
    laser->kal.cov = 1;
}

float laser_get_dist(laser_handle_t laser)
{
    return laser->dist;
}